//
// Simple example to demonstrate how takeoff and land using MAVSDK.
//

#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <signal.h>
#include <iostream>

#include "discovery_service.h"
#include "softbus_bus_center.h"
#include "session.h"

#define pkgName "test2"
#define sessionName "test2"
static int g_publishID = 123;
static const char cData[] = "hello world test";

#include "vehicle.h"
#include <thread>

using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;

#define TMP_PATH "/tmp/softbus_recv_pid.tmp"
#define CMD_TAKEOFF 0x10
#define CMD_LAND 0x20
#define SIG_TYPE SIGUSR2

Vechile vechile("udp://127.0.0.1:10000");
bool flag_stop = false;

void DeviceFound(const DeviceInfo *device)
{
	int i;
	printf("<DeviceFound>CB: Device has found\n");
	printf("\tdevId=%s\n", device->devId);
	printf("\tdevName=%s\n", device->devName);
	printf("\tdevType=%d\n", device->devType);
	printf("\taddrNum=%d\n", device->addrNum);
	for (i = 0; i < device->addrNum; i++)
	{
		printf("\t\taddr%d:type=%d,", i + 1, device->addr[i].type);
		switch (device->addr[i].type)
		{
		case CONNECTION_ADDR_WLAN:
		case CONNECTION_ADDR_ETH:
			printf("ip=%s,port=%d,", device->addr[i].info.ip.ip, device->addr[i].info.ip.port);
			break;
		default:
			break;
		}
		printf("peerUid=%s\n", device->addr[i].peerUid);
	}
	printf("\tcapabilityBitmapNum=%d\n", device->capabilityBitmapNum);
	for (i = 0; i < device->addrNum; i++)
	{
		printf("\t\tcapabilityBitmap[%d]=0x%x\n", i + 1, device->capabilityBitmap[i]);
	}
	printf("\tcustData=%s\n", device->custData);
}

void DiscoverySuccess(int subscribeId)
{
	printf("<DiscoverySuccess>CB: discover subscribeId=%d\n", subscribeId);
}

void DiscoveryFailed(int subscribeId, DiscoveryFailReason reason)
{
	printf("<DiscoveryFailed>CB: discover subscribeId=%d failed, reason=%d\n", subscribeId, (int)reason);
}

int DiscoveryInterface()
{
	SubscribeInfo info = {
		.subscribeId = g_publishID,
		.mode = DISCOVER_MODE_ACTIVE,
		.medium = COAP,
		.freq = LOW,
		.isSameAccount = false,
		.isWakeRemote = false,
		.capability = "osdCapability",
		.capabilityData = (unsigned char *)cData,
		.dataLen = strlen(cData) + 1,
	};
	IDiscoveryCallback cb = {
		.OnDeviceFound = DeviceFound,
		.OnDiscoverFailed = DiscoveryFailed,
		.OnDiscoverySuccess = DiscoverySuccess,
	};
	return StartDiscovery(pkgName, &info, &cb);
}

int SessionOpened(int sessionId, int result)
{
	printf("<SessionOpened>CB: session %d open ret=%d\n", sessionId, result);
	return 0;
}

void SessionClosed(int sessionId)
{
	printf("<SessionClosed>CB: session %d closed\n", sessionId);
}

void ByteRecived(int sessionId, const void *data, unsigned int dataLen)
{
	printf("<ByteRecived>CB: session %d received %u bytes data=%s\n", sessionId, dataLen, (const char *)data);
}

void MessageReceived(int sessionId, const void *data, unsigned int dataLen)
{
	printf("<MessageReceived>CB: session %d received %u bytes message=%s\n", sessionId, dataLen, (const char *)data);
}

const ISessionListener sessionCB = {
	.OnSessionOpened = SessionOpened,
	.OnSessionClosed = SessionClosed,
	.OnBytesReceived = ByteRecived,
	.OnMessageReceived = MessageReceived,
};

int OpenSessionInterface(const char *SessionName, const char *peerName, const char *peerId)
{
	SessionAttribute attr = {
		.dataType = TYPE_BYTES,
		.linkTypeNum = 1,
		.attr = {RAW_STREAM},
	};
	LinkType lt = LINK_TYPE_WIFI_WLAN_2G;
	attr.linkType[0] = lt;
	return OpenSession(SessionName, peerName, peerId, "MyGroup", &attr);
}

void GetAllNodeDeviceInfoInterface(NodeBasicInfo **dev, int32_t *num)
{
	GetAllNodeDeviceInfo(pkgName, dev, num);
	printf("<GetAllNodeDeviceInfo>return %d Node\n", *num);
	for (int i = 0; i < *num; i++)
	{
		printf("<Node %d>deviceName=%s, networkId=%s, Type=%uud\n", i + 1, dev[i]->deviceName, dev[i]->networkId, dev[i]->deviceTypeId);
	}
}

void WritePid()
{
	int pid = getpid();
	char pid_buffer[20];
	sprintf(pid_buffer, "%d", pid);
	FILE *fd = fopen(TMP_PATH, "w"); // w模式删除原内容

	//此处已经做了文件不存在状态下的处理办法
	if (fd != NULL)
	{
		fwrite(pid_buffer, 1, 20, fd);
		fclose(fd);

		pid = atoi(pid_buffer);
	}
	return;
}

void SignalHandle(int signum, siginfo_t *info, void *context) //信号处理函数
{
	if (info->si_value.sival_int == 0x10)
	{
		printf("[CMD]get cmd takeoff\n");
		vechile.takeoff();
	}
	else if (info->si_value.sival_int == 0x20)
	{
		printf("[CMD]get cmd land\n");
		vechile.land();
	}
}
void myhandler(int sig)
{
	flag_stop = true;
}

int main(int argc, char **argv)
{
	// softbus ret
	Vechile vechile("udp://127.0.0.1:10000");
	vechile.disarm();
	vechile.arm();

	char cmd;
	for(;;){
		scanf("%c", &cmd);
		if (cmd=='t'){
	
				printf("takeoff\n");
				vechile.takeoff();
		}else if (cmd=='l'){
				printf("land\n");
					vechile.land();
		}else if(cmd=='r'){
				printf("read\n");
				vechile.readTelemetry().dump();
		}else if (cmd=='d'){
				printf("disarm\n");
				vechile.disarm();
			}else if (cmd=='a'){
				printf("arm\n");
					vechile.arm();
			}
	}
return 0;

	int ret;
	NodeBasicInfo *dev;
	int32_t dev_num;
	bool needContinue = true;
	char dData[350];
	int times = 0;
	//注册信号软中断
	struct sigaction act;						   //这个结构体声明了一种处理办法
	act.sa_sigaction = SignalHandle;			   // sig结构体的处理函数
	act.sa_flags = SA_RESTART | SA_SIGINFO;		   // SA_SIGINFO指定使用sa_sigaction作为默认的毁掉函数句柄，否则使用sa_handler
												   // SA_RESTART允许系统调用重新发起，不是很懂，去掉目前看不出来影响
	int sig_ret = sigaction(SIG_TYPE, &act, NULL); //核心，将act结构体与sigusr1关联，注册中断，注册失败ret会返回负值
	if (sig_ret < 0)
	{
		printf("[ERROR]sig注册失败\n");
		return 0;
	}
	//写入pid供send端查看
	signal(SIGINT, myhandler);

	//链接飞控
	vechile.arm();
	printf("softbus start\n");

	//软总线初始化
	ret = DiscoveryInterface();
	if (ret)
	{
		printf("DiscoveryInterface fail, ret=%d\n", ret);
		goto err_DiscoveryInterface;
	}
	ret = CreateSessionServer(pkgName, sessionName, &sessionCB);
	if (ret)
	{
		printf("CreateSessionServer fail, ret=%d\n", ret);
		goto err_CreateSessionServer;
	}

	GetAllNodeDeviceInfoInterface(&dev, &dev_num);
	if (dev_num)
	{
		int sessionId = OpenSessionInterface(sessionName, sessionName, dev[0].networkId);
		if (sessionId < 0)
		{

			printf("OpenSessionInterface fail, ret=%d\n", sessionId);
			return 0;
		}
		//软总线session已建立，唯一sessionID
		WritePid();

		while (needContinue)
		{

			strcpy(dData, vechile.readTelemetry().toJson().c_str());

			SendBytes(sessionId, dData, strlen(dData) + 1);

			printf("send data,%d\n", times++);
			if (flag_stop == true)
				break;
			usleep(30000);
		}

		CloseSession(sessionId);
	}
	else
	{
		printf("Get no Node\n");
	}

	// vechile.readTelemetry();

	//软总线关闭
	RemoveSessionServer(pkgName, sessionName);
err_CreateSessionServer:
	StopDiscovery(pkgName, g_publishID);
err_DiscoveryInterface:
	return 0;
}
